#include "headfile.h"

pid_t motorA;
pid_t motorB;
pid_t angle;
pid_t positionA;
pid_t positionB;
float TempA, TempB;
int SpeedA_reference = 20; // 基准速度
int SpeedB_reference = 20; // 基准速度
int Expand;
float target_angle = -38.66; //目标角度，在白线时依靠此值走直线
float angle_diff;



void pid_init(pid_t *pid, uint32_t mode, float p, float i, float d) // 设定PID值
{
    pid->pid_mode = mode;
    pid->p = p;
    pid->i = i;
    pid->d = d;
}

void pid_cal(pid_t *pid) // 计算PID输出,在中断中使用
{
    // 计算当前偏差
    pid->error[0] = pid->target - pid->now;
    if (pid->error[0] < 0.3 && pid->error[0] > -0.3)
    {
        pid->error[0] = 0;
    }

    // 计算输出
    if (pid->pid_mode == DELTA_PID) // 增量式
    {
        pid->pout = pid->p * (pid->error[0] - pid->error[1]);
        pid->iout = pid->i * pid->error[0];
        pid->dout = pid->d * (pid->error[0] - 2 * pid->error[1] + pid->error[2]);
        pid->out += pid->pout + pid->iout + pid->dout;
    }
    else if (pid->pid_mode == POSITION_PID) // 位置式
    {
        pid->pout = pid->p * pid->error[0];
        pid->iout += pid->i * pid->error[0];
        pid->dout = pid->d * (pid->error[0] - pid->error[1]);
        pid->out = pid->pout + pid->iout + pid->dout;
    }

    // 记录前两次偏差
    pid->error[2] = pid->error[1];
    pid->error[1] = pid->error[0];
}



/// @brief speed loop target set
/// @param spe1 motorA speed
/// @param spe2 motorB speed
void speed_target(int spe1, int spe2)
{
    if (spe1 >= 0)
    {
        motorA_dir = 1;
        motorA.target = spe1;
    }
    else
    {
        motorA_dir = 0;
        motorA.target = -spe1;
    }

    if (spe2 >= 0)
    {
        motorB_dir = 0;
        motorB.target = spe2;
    }
    else
    {
        motorB_dir = 1;
        motorB.target = -spe2;
    }
}

/// @brief Get encoder value, read data every 1ms
/// @param  void
// int speed_now(int dir)
// {
//     if (!dir)
//         return motorA.now ;  // 返回编码器A处理后的值
//         return motorB.now ;      // 返回编码器B处理后的值
//     // return Encoder_A.Obtained_Get_Encoder_Count;  // 返回编码器A处理后的值
//     //return Encoder_B.Obtained_Get_Encoder_Count;      // 返回编码器B处理后的值
// }

int speed_now(int dir)
{
    if (!dir)
    {
        return motorA.now ;  // 返回编码器A处理后的值
    }
    else
    {
        return motorB.now ;  // 返回编码器B处理后的值
    }
}


/// @brief speed out and input motor
/// @param  void
void speed_out(void)
{
    pid_cal(&motorA);
    pid_cal(&motorB);
    pidout_limit(&motorA);
    pidout_limit(&motorB);
    MotorA_Set_Duty(motorA.out);
    MotorB_Set_Duty(motorB.out);
}

/// @brief get position loop now value,it must follow after obtaining the speed value
/// @param  void
void position_now(void)
{
    positionA.now += motorA.now;
    positionB.now += motorB.now;
}

/// @brief calculate position loop output
/// @param void
void position_out(void)
{
    // 计算PID输出
    pid_cal(&positionA);
    pid_cal(&positionB);
    // 输出映射
    positionA.out = mapValue(positionA.target, positionA.out);
    positionB.out = mapValue(positionB.target, positionB.out);
}

/// @brief set position loop target/转一圈/编码器读数为40000/减速比20 *编码器精度500 * 4倍频 = 40000
void position_target(float circle)
{
    positionA.target = positionA.now + (-40000 * circle);
    positionB.target = positionB.now + (-40000 * circle);
}

void position_default(void)
{
    positionA.target = positionA.now;
    positionB.target = positionB.now;
}

/**
 * 将输入值映射到-1000到1000范围,用于限制位置环输出
 * @param maxValue 原始范围的最大值（绝对值代表范围上限）
 * @param value 需要映射的值
 * @return 映射后-1000到1000范围内的值
 */
int mapValue(int maxValue, int value)
{
    // 处理边界情况：最大值为0时直接返回0
    if (maxValue == 0)
    {
        return 0;
    }

    // 计算映射系数（考虑符号）
    int sign = (maxValue > 0) ? 1 : -1;
    int absMax = abs(maxValue);

    // 线性映射计算：value * 1000 / absMax，符号由maxValue决定
    int mappedValue = (value * 1000 * sign) / absMax;

    // 确保结果在-1000到1000范围内
    if (mappedValue < -1000)
    {
        mappedValue = -1000;
    }
    else if (mappedValue > 1000)
    {
        mappedValue = 1000;
    }

    return mappedValue;
}
/// @brief obtain current angle value through MPU6050
/// @param  void
// void angle_now(void)
// {
//     angle.now = yaw;
// }

void angle_out(void)
{
    pid_cal(&angle);
}

/// @brief set angle loop target
/// @param target target anglr value
void angle_target(float target)
{
    angle_turn(target);
    angle.target = target;
}

/// @brief limit the intput angle when changing target value of angle-loop
/// @param angle
/// @return
float angle_turn(float angle)
{
    if (angle >= 180)
    {
        int Temp = angle;
        angle = -(Temp - 180);
    }
    if (angle <= -180)
    {
        int Temp = angle;
        angle = -(Temp + 180);
    }
    return angle;
}

/// @brief set all loop pid value
/// @param  void
void pid_setValue(void)
{
    pid_init(&motorA, DELTA_PID, 5, 1, 0.4);
    pid_init(&motorB, DELTA_PID, 7, 1, 0.1);
    pid_init(&angle, POSITION_PID, 2, 0, 1.7);
    pid_init(&positionA, POSITION_PID, 1, 0, 0);
    pid_init(&positionB, POSITION_PID, 1, 0, 0);
}


void PID_Overall_Contral(uint8_t status)
{
    
    //+-是向前
    
    pid_setValue();


    switch (status)
    {
    case ANGLE_SPEED:
    {
        
        // if(Switch_flag==0)
        // {
        //     angle_diff = target_angle - yaw;
        //     Akm_cal_A(0.2, angle_diff);
        // }
        // if(Switch_flag==2)
        // {
        //     angle_diff = -target_angle + yaw;
        //     Akm_cal_A(0.2, angle_diff);
        // }
        angle_diff = target_angle - yaw;
        Akm_cal_A(0.2, angle_diff);
        TempA = Pulse_V(Target_A);
        TempB = -Pulse_V(Target_B);
        speed_target(TempA, TempB);
        Motor_dir(motorA_dir, motorB_dir);
        Servo_Set_Angle(Servo);
        speed_now(1);
        speed_now(0);
        speed_out();
        break;
    }
    case POSITION_SPEED:
    {
        if (positionA.out > 10)
            TempA = SpeedA_reference;
        else
            TempA = 0;
        if (positionA.out > 10)
            TempB = SpeedB_reference;
        else
            TempB = 0;
        speed_target(TempA, TempB);
        Motor_dir(motorA_dir, motorB_dir);
        speed_now(0);
        speed_now(1);
        position_now();
        speed_out();
        position_target(1.82);
        position_out();
        break;
    case TRACK_SPEED:
    {
        if(Expand>30)Expand=30;
        if(Expand<-30)Expand=-30;
        Akm_cal_A(0.1, Expand);
        TempA = Pulse_V(Target_A);
        TempB = -Pulse_V(Target_B);
        speed_target(TempA, TempB);
        Motor_dir(motorA_dir, motorB_dir);
        Servo_Set_Angle(Servo);
        speed_now(1);
        speed_now(0);
        speed_out();
        break;
    }
    case STOP:
    {
        TempA = 0;
        TempB = 0;
        speed_target(TempA, TempB);
        Servo_Set_Angle(90);
        Motor_dir(motorA_dir, motorB_dir);
        speed_now(1);
        speed_now(0);
        speed_out();
        break;
    }
    

    }
    default:
        break;
    }

    // 状态机
    // switch (status)
    // {
    // case 0:
    // {
    //     position_Contral();
    //     if (positionA.out > 10)
    //         TempA = 300;
    //     else
    //         TempA = 0;
    //     if (positionA.out > 10)
    //         TempB = 300;
    //     else
    //         TempB = 0;
    // }
    // case 1:
    // {
    //     position_Contral();
    //     if (positionA.out > 10)
    //         TempA = -300;
    //     else
    //         TempA = 0;
    //     if (positionA.out > 10)
    //         TempB = -300;
    //     else
    //         TempB = 0;
    // }
    // case 2:
    // {
    //     position_Contral();
    //     if (positionA.out > 10)
    //         TempA = 300;
    //     else
    //         TempA = 0;
    //     if (positionA.out > 10)
    //         TempB = 300;
    //     else
    //         TempB = 0;
    // }
    // case 3:
    // {
    //     TempA = (Pulse_targetA * 3) + Expand * (Pulse_targetA * 3 / 15);
    //     TempB = -((Pulse_targetB * 3) - Expand * (Pulse_targetB * 3 / 15));
    // }
    // }

    //    if (Turn_status == 0) // 未转向
    //    {
    //        TempA = (Pulse_targetA * 3) + Expand * (Pulse_targetA * 3 / 15);
    //        TempB = -((Pulse_targetB * 3) - Expand * (Pulse_targetB * 3 / 15));
    //    }
    // if (Turn_status == 1 && Lturn_90 == 1) // zuo转向中
    // {
    //     TempA = -(10 * 40);
    //     TempB = -(10 * 40);
    // }
    // if (Turn_status == 1 && Rturn_90 == 1) // you转向中
    // {
    //     TempA = (10 * 40);
    //     TempB = (10 * 40);
    // }
    // if (stop_flag == 1)
    // {
    //     TempA = 0;
    //     TempB = 0;
    // }
    //+-是向前，++是右转,--是左转

    // 速度环
    // 1.设定目标速度
    // motor_target_set(TempA, TempB);
    // Motor_dir(motorA_dir, motorB_dir);
    // //    // 2.获取当前速度
    // Pulse_Count_Get();
    // positionA.now += Pulse_Count_A;
    // positionB.now += Pulse_Count_B;
    // //    // 3.输入PID控制器进行计算
    // pid_cal(&motorA);
    // pid_cal(&motorB);
    // //    // 电机输出限幅
    // pidout_limit(&motorA);
    // pidout_limit(&motorB);
    // //    // 4.PID的输出值 输入给电机
    // MotorA_Set_Duty(motorA.out);
    // MotorB_Set_Duty(motorB.out);
}

//PWM输出限幅
void pidout_limit(pid_t *pid)
{
    // 输出限幅
    if (pid->out >= 999)
        pid->out = 999;
    if (pid->out <= 0)
        pid->out = 0;
}

/// @brief obtain current angle value through MPU6050
/// @param  void
void angle_now(void)
{
    //angle.now = yaw;
}

